/*
    功能实现
*/

#include "Arduino.h"
#include "Joystick.h"
#include "f.h"


/*-----Joystick-----*/
//模拟值映射的最小值、中间值、最大值
#define MIN_VALUE 0
#define MAX_VALUE 1024
#define MID_VALUE (MIN_VALUE+MAX_VALUE)/2

Joystick_ Joystick(JOYSTICK_DEFAULT_REPORT_ID, //IP
    JOYSTICK_TYPE_MULTI_AXIS, BUTTON_NUM, 0,//手柄的模式，按键数量，hat switch数量
    false, false, false, //xyz
    false, false, false, //Rxyz
    false, false, //舵，节流阀
    true, true, true);//加速，刹车，转向

bool stateChangedFlag=1;

void initJoystick(){
    pinMode(LED_RETURN, OUTPUT);
    digitalWrite(LED_RETURN, LOW);

    Joystick.setAcceleratorRange(MIN_VALUE, MAX_VALUE);
    Joystick.setBrakeRange(MIN_VALUE, MAX_VALUE);
    Joystick.setSteeringRange(MIN_VALUE, MAX_VALUE);
    if(AUTO_SEND)
        Joystick.begin();
    else
        Joystick.begin(false);

    Joystick.setAccelerator(MID_VALUE);
    Joystick.setBrake(MID_VALUE);
    Joystick.setSteering(MID_VALUE);
    stateChangedFlag=true;
}

void sendState(){
    if(stateChangedFlag==true && AUTO_SEND==0)
    {
        stateChangedFlag=false;
        Joystick.sendState();
    }
}

void DEBUG_FUN(){
    uint16_t val;
    char str[15];

    analogRead(GND_PIN);
    val = analogRead(STEERING_PIN);
    sprintf(str,"steering:%4d",val);
    Serial.print(str);

    analogRead(GND_PIN);
    val = analogRead(SPEED_PIN);
    sprintf(str,"  speed:%4d",val);
    Serial.print(str);

    analogRead(GND_PIN);
    val = analogRead(BRAKE_PIN);
    sprintf(str,"  brake:%4d",val);
    Serial.print(str);

    for(uint8_t i=0;i<BUTTON_NUM;i++)
    {
        sprintf(str,"  %2d:%d",i+1,!digitalRead(BUTTON_BEGIN+i));
        Serial.print(str);
    }

    sprintf(str,"  switch:%d",!digitalRead(DIRECTION_SWITCH_PIN));
    Serial.print(str);

    Serial.println();
    delay(1000);
}

/*-----按键类-----*/
void initButtons(){
    for(uint8_t i=0;i<BUTTON_NUM;i++)//普通手柄按键，闭合按下
    {
        pinMode(BUTTON_BEGIN+i,INPUT_PULLUP);
    }
    pinMode(DIRECTION_SWITCH_PIN,INPUT_PULLUP);//前进方向控制，闭合前进
}

bool button_state[BUTTON_NUM] = {0};
void readButtonsAndSet(){
    bool state;
    for(uint8_t i=0;i<BUTTON_NUM;i++)
    {
        state = !digitalRead(BUTTON_BEGIN+i);
        if(state!=button_state[i])//按键状态发生变化
        {
            stateChangedFlag=true;
            button_state[i]=state;
            if(state==1)
                Joystick.pressButton(i);
            else
                Joystick.releaseButton(i);
        }
    }
}


/*-----模拟量类-----*/
bool isSimilar(uint16_t a, uint16_t b, uint16_t accuracy){
    uint16_t difference;
    difference = (a>b)?a-b:b-a;
    return difference < accuracy;
}

uint16_t steering_value=MID_VALUE;
void readSteeringAndSet(){
    analogRead(GND_PIN);
    uint16_t val=analogRead(STEERING_PIN);

    //尝试将val吸附到左极值或中值或右极值，然后映射
    if(isSimilar(val, STEERING_MIDDLE, STEERING_DEADZONE_MIDDLE))
        val=MID_VALUE;
    else if(isSimilar(val,STEERING_LEFT,STEERING_DEADZONE_LEFT))
        val=MIN_VALUE;
    else if(isSimilar(val,STEERING_RIGHT,STEERING_DEADZONE_RIGHT))
        val=MAX_VALUE;
    else//吸附失败，直接进行映射
    {
        if(val<STEERING_MIDDLE)//向左映射
        {
            val=map(val, STEERING_LEFT+STEERING_DEADZONE_LEFT, STEERING_MIDDLE-STEERING_DEADZONE_MIDDLE, MIN_VALUE, MID_VALUE-STEERING_SENSITIVITY);
        }
        else//向右映射
        {
            val=map(val, STEERING_MIDDLE+STEERING_DEADZONE_MIDDLE, STEERING_RIGHT-STEERING_DEADZONE_RIGHT, MID_VALUE+STEERING_SENSITIVITY, MAX_VALUE);
        }
    }

    //回正指示灯
    if(val==MID_VALUE)
        digitalWrite(LED_RETURN, HIGH);
    else
        digitalWrite(LED_RETURN, LOW);

    //查看是否需要上传
    if(!isSimilar(steering_value, val, STEERING_SENSITIVITY))
    {
        stateChangedFlag=true;
        steering_value=val;
        Joystick.setSteering(val);
    }
}

uint16_t speed_value=MID_VALUE;
void readSpeedAndSet(){
    analogRead(GND_PIN);
    uint16_t val=analogRead(SPEED_PIN);
    bool direction=!digitalRead(DIRECTION_SWITCH_PIN);

    //尝试将val吸附到起始值或终止值
    if(isSimilar(val,SPEED_RELEASE,SPEED_DEADZONE_RELEASE))
        val=MID_VALUE;
    else if(isSimilar(val,SPEED_PRESS,SPEED_DEADZONE_PRESS))
        val=direction?MAX_VALUE:MIN_VALUE;
    else//吸附失败，直接进行映射
    {
        if(direction)//前进
        {
            val=map(val, SPEED_RELEASE+SPEED_DEADZONE_RELEASE, SPEED_PRESS-SPEED_DEADZONE_PRESS, MID_VALUE+SPEED_SENSITIVITY, MAX_VALUE);
        }
        else//后退
        {
            val=map(val, SPEED_RELEASE+SPEED_DEADZONE_RELEASE, SPEED_PRESS-SPEED_DEADZONE_PRESS, MID_VALUE-SPEED_SENSITIVITY, MIN_VALUE);
        }
    }
    
    //查看是否需要上传
    if(!isSimilar(speed_value, val, SPEED_SENSITIVITY))
    {
        stateChangedFlag=true;
        speed_value=val;
        Joystick.setAccelerator(val);
    }
}

uint16_t brake_value=MID_VALUE;
void readBrakeAndSet(){
    analogRead(GND_PIN);
    uint16_t val=analogRead(BRAKE_PIN);

    //尝试将val吸附到起始值或终止值，brake值越小越刹车
    if(isSimilar(val, BRAKE_RELEASE, BRAKE_DEADZONE_RELEASE))
        val=MID_VALUE;
    else if(isSimilar(val, BRAKE_PRESS,BRAKE_DEADZONE_PRESS))
        val=MIN_VALUE;
    else//吸附失败，直接进行映射
    {
        val=map(val,BRAKE_RELEASE+BRAKE_DEADZONE_RELEASE,BRAKE_PRESS-BRAKE_DEADZONE_PRESS,MID_VALUE-BRAKE_SENSITIVITY,MIN_VALUE);
    }

#ifndef BRAKE_MAPPING
    //未启用刹车映射时的程序
    if(!isSimilar(brake_value,val,BRAKE_SENSITIVITY))
    {
        stateChangedFlag=true;
        brake_value=val;

        #ifndef BRAKE_MAPPING2SPEED
        Joystick.setBrake(val);
        #else
        Joystick.setAccelerator(val);
        #endif
    }
#else
    //启用了刹车映射时的程序
    if(val!=MID_VALUE && brake_value==MID_VALUE)//现在做了刹车动作，但之前处于未刹车状态
    {
        stateChangedFlag=true;
        brake_value=val;
        Joystick.pressButton(BRAKE_MAPPING-1);
    }
    else if(val==MID_VALUE && brake_value!=MID_VALUE)//现在释放了刹车，但之前处于刹车状态
    {
        stateChangedFlag=true;
        brake_value=val;
        Joystick.releaseButton(BRAKE_MAPPING-1);
    }
#endif
}